#include "bsp/bsp.h"
#include "gpio.h"

#define N_EXTI 16

struct subscription_t {
    u32  port;
    void (*callback)(void);
    void* ctx;
} subscriptions[N_EXTI];

static u8 clk_enabled_[PORT_MAX] = {0};

static __INLINE u16 get_pin_number(u16 pin_mask_) {
	uint16_t pin_number = 0;
	uint16_t pin_mask = pin_mask_ >> 1;
	while (pin_mask) {
		pin_mask >>= 1;
		pin_number++;
	}
	return pin_number;
}

static inline IRQn_Type get_irq_number(u16 pin_number) {
    switch (pin_number) {
        case 0: return EXTI0_IRQn;
        case 1: return EXTI1_IRQn;
        case 2: return EXTI2_IRQn;
        case 3: return EXTI3_IRQn;
        case 4: return EXTI4_IRQn;
        case 5:
        case 6:
        case 7:
        case 8:
        case 9: return EXTI5_9_IRQn;
        case 10:
        case 11:
        case 12:
        case 13:
        case 14:
        case 15: return EXTI10_15_IRQn;
        default: return (IRQn_Type)0; // impossible
    }
}


static inline u32 get_gpio_port(u32 io) {
	u16 io_port = PORT(io);
    switch (io_port) {
        case 0: return GPIOA;
        case 1: return GPIOB;
        case 2: return GPIOC;
        case 3: return GPIOD;
        case 4: return GPIOE;
        case 5: return GPIOF;
        case 6: return GPIOG;
        default: return 0; // impossible
    }
}

static void _gpio_enable_clk(u32 io) {
  	u32 port = get_gpio_port(io);
	if (port == 0) {
		return;
	}
    if (port == GPIOA) {
        rcu_periph_clock_enable(RCU_GPIOA);
		clk_enabled_[0] = 1;
    } else if (port == GPIOB) {
        rcu_periph_clock_enable(RCU_GPIOB);
		clk_enabled_[1] = 1;
    } else if (port == GPIOC) {
        rcu_periph_clock_enable(RCU_GPIOC);
		clk_enabled_[2] = 1;
    } else if (port == GPIOD) {
        rcu_periph_clock_enable(RCU_GPIOD);
		clk_enabled_[3] = 1;
    } else if (port == GPIOE) {
        rcu_periph_clock_enable(RCU_GPIOE);
		clk_enabled_[4] = 1;
    } else if (port == GPIOF) {
        rcu_periph_clock_enable(RCU_GPIOF);
		clk_enabled_[5] = 1;
    } else if (port == GPIOG) {
        rcu_periph_clock_enable(RCU_GPIOG);
		clk_enabled_[6] = 1;
    }else {
        return;
    }
}

void _gpio_init(u32 io, u32 mode, u32 speed) {
	if (io != INVALD_PIO) {
  		uint32_t port = get_gpio_port(io);
		if (port == 0) {
			return;
		}
		_gpio_enable_clk(io);
		gpio_init(port, mode, speed, PIN(io));
	}
}


void gpio2_init(u32 io, u32 mode, u32 speed) {
    _gpio_init(io, mode, speed);
}

void gpio2_analog_init(u32 io) {
    gpio2_init(io, GPIO_MODE_AIN, GPIO_OSPEED_10MHZ);
}


void gpio2_af_init(u32 io, u32 mode, u32 speed, u32 remap) {
	if (io != INVALD_PIO) {
    	_gpio_init(io, mode, speed);
		if (remap != 0xFFFFFFFF) {
			gpio_pin_remap_config(remap, ENABLE);
		}
	}
}

void gpio2_deinit(u32 io) {

}

void gpio2_write(u32 io, bool set) {
	if (io != INVALD_PIO) {
		if (clk_enabled_[PORT(io)] == 0) {
			_gpio_enable_clk(io);
		}
		gpio_bit_write(get_gpio_port(io), PIN(io), set ? SET : RESET);
	}
}

bool gpio2_read(u32 io) {
	if (io == INVALD_PIO) {
		return false;
	}
	return (GPIO_ISTAT(get_gpio_port(io)) & PIN(io)) != RESET;
}

#define io_2_exitline(io) (exti_line_enum)(PIN(io))

void exti_subscribe(u32 io, u32 edge, exti_handler handler) {
	exti_trig_type_enum trig;
	if (edge == EXINT_TRIG_ALL) {
		trig = EXTI_TRIG_BOTH;
	}else if (edge == EXINT_TRIG_RAISING) {
		trig = EXTI_TRIG_RISING;
	}else {
		trig = EXTI_TRIG_FALLING;
	}

	gpio2_init(io, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_10MHZ);

	u16 pin_number = get_pin_number(PIN(io));
	IRQn_Type n_irq = get_irq_number(pin_number);
	if (n_irq == 0 || pin_number >= N_EXTI) {
		return;
	}
	__disable_irq();
	subscriptions[pin_number].callback = handler;
	subscriptions[pin_number].ctx = NULL;
	subscriptions[pin_number].port = NULL;
	__enable_irq();

	gpio_exti_source_select(PORT(io), pin_number);
	exti_init(io_2_exitline(io), EXTI_INTERRUPT, trig);
	nvic_irq_enable(n_irq, 0, 0U);
	exti_interrupt_flag_clear(io_2_exitline(io));
	exti_interrupt_enable(io_2_exitline(io));

}
void exti_unsubscribe(u32 io) {
	exti_interrupt_flag_clear(io_2_exitline(io));
	exti_interrupt_disable(io_2_exitline(io));
	uint16_t pin_number = get_pin_number(PIN(io));
	if (pin_number >= N_EXTI) {
		return;
	}
	subscriptions[pin_number].callback = NULL;
	subscriptions[pin_number].ctx = NULL;
	subscriptions[pin_number].port = NULL;
}

static __INLINE void exti_handle(u16 exti_number) {
    if(exti_interrupt_flag_get((exti_line_enum)(1 << exti_number)) == RESET) {
        return; // This interrupt source did not trigger the interrupt line
    }

    exti_interrupt_flag_clear((exti_line_enum)(1 << exti_number));

    struct subscription_t *subscription = &subscriptions[exti_number];
    if (subscription->callback) {
        (subscription->callback)();
    }
}

bool exti_is_pending_clear(u32 io) {
	if (exti_interrupt_flag_get(io_2_exitline(io)) == SET){
		exti_interrupt_flag_clear(io_2_exitline(io));
		return true;
	}
	return false;
}

/** @brief Entrypoint for the EXTI line 0 interrupt. */
void EXTI0_IRQHandler(void) {
    exti_handle(0);
}

/** @brief Entrypoint for the EXTI line 1 interrupt. */
void EXTI1_IRQHandler(void) {
    exti_handle(1);
}

/** @brief Entrypoint for the EXTI line 2 interrupt. */
void EXTI2_IRQHandler(void) {
    exti_handle(2);
}

/** @brief Entrypoint for the EXTI line 3 interrupt. */
void EXTI3_IRQHandler(void) {
    exti_handle(3);
}

/** @brief Entrypoint for the EXTI line 4 interrupt. */
void EXTI4_IRQHandler(void) {
    exti_handle(4);
}

/** @brief Entrypoint for the EXTI lines 5-9 interrupt. */
void EXTI5_9_IRQHandler(void) {
    exti_handle(5);
    exti_handle(6);
    exti_handle(7);
    exti_handle(8);
    exti_handle(9);
}

/** @brief This function handles EXTI lines 10-15 interrupt. */
void EXTI10_15_IRQHandler(void) {
    exti_handle(10);
    exti_handle(11);
    exti_handle(12);
    exti_handle(13);
    exti_handle(14);
    exti_handle(15);
}

